#include "rosconnector.h"
#include <math.h>

RosConnector::RosConnector(ros::NodeHandle &nh, Widget &w)
{
    info_to_april.header.frame_id = "usb_cam";
    info_to_april.height = 480;
    info_to_april.width = 640;
    info_to_april.distortion_model = "plumb_bob";
    info_to_april.D = {0, 0, 0, 0, 0};
    info_to_april.K = {443.4, 0, 256, 0, 443.4, 256, 0, 0, 1};
    info_to_april.R = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
    info_to_april.P = {443.4, 0.0, 256, 0.0, 0.0, 443.4, 256, 0.0, 0.0, 0.0, 1.0, 0.0};
    info_to_april.binning_x = 0;
    info_to_april.binning_y = 0;
    info_to_april.roi.x_offset = 0;
    info_to_april.roi.y_offset = 0;
    info_to_april.roi.height = 0;
    info_to_april.roi.width = 0;
    info_to_april.roi.do_rectify = false;   

    apriltag_corpoint.resize(32);
    
    motors.resize(11);
    bulk_msg_port4.id1=1;       //推杆舵机        //舵机顺序
    bulk_msg_port4.id2=2;       //1关节舵机
    bulk_msg_port3.id1=3;       //4关节舵机
    bulk_msg_port3.id2=4;       //5关节舵机
    bulk_msg_port3.id3=5;       //6关节舵机
    bulk_msg_port4.item1="position";
    bulk_msg_port4.item2="position";
    bulk_msg_port3.item1="position";
    bulk_msg_port3.item2="position";
    bulk_msg_port3.item3="position";
    rate_f=30;

    i_coverPlate=0;
    i_operate=0;
    i_screw=0;
    lift_start = 0;

    motor_limit_value={{45,90.1},
                        {0,25},
                        {40,140},
                        {-100,100},
                        {45,140},
                        {-45,45}
                        };    
    
    maxon_msg.data.resize(4);
    total_msg.data.resize(8);
    present_position.resize(11);
    joint_simulation_position.resize(6);
    joint_simulation_position_rec.resize(6);

    cartesian_position.resize(10);
    for (int i=0;i<10;i++){
        cartesian_position[i].resize(4);
        for (int j=0;j<4;j++){
            cartesian_position[i][j].resize(8);
            for (int k=0;k<8;k++){
                cartesian_position[i][j][k].resize(3);
            }
        }
    }
    cartesian_orientation.resize(10);
    for (int i=0;i<10;i++){
        cartesian_orientation[i].resize(4);
        for (int j=0;j<4;j++){
            cartesian_orientation[i][j].resize(8);
            for (int k=0;k<8;k++){
                cartesian_orientation[i][j][k].resize(3);
            }
        }
    }

    // cartesian_position={{{{645.67,465.93,0},{629.51,504.98,32.12}},{{629.51,504.98,32.12},{629.51,504.98, -27.88}}},
    //                     {{{536.65,575.08,0},{538.65,584.98,1.66}},{{538.65,584.98 ,1.66},{485.65, 584.98, 1.66}}},
                        
    //                     {{{536.65,575.08,0},{533.65,584.98,-0.34}},{{533.65,584.98,-0.34},{514.15, 584.98, -157.84}},{{514.15, 584.98, -157.84},{454.15, 584.98, -157.84}}},
    //                     {{{594.66,571.27,0},{650.92,576.81,-10.29}},{{650.92,576.81,-10.29},{579.12,576.81,17 }},{{579.12,576.81,17 },{472.4,654.81,174.5}}},

    //                     {{{601.13,529.75,0},{644.15,596.81,97}},{{644.15,596.81,97},{579.12, 596.81,97}},{{579.12,596.81,97},{514.15,596.81,97}},{{514.15,596.81,97},{454.15,596.81,97}}},
    //                     {{{601.13,529.75,0},{629.51,504.98,-32.12}},{{629.51,504.98,-32.12},{629.51,504.98,27.88}}},
    //                     {{{536.65,575.08,0},{538.65,584.98,-1.66}},{{538.65,584.98,-1.66},{485.65, 584.98, -1.66}}},
                        
    //                     {{{536.65,575.08,0},{533.65,584.98,0.34}},{{533.65,584.98,0.34},{514.15, 584.98, 157.84}},{{514.15, 584.98, 157.84},{454.15, 584.98,157.84}}},
    //                     {{{594.66,571.27,0},{472.4,654.81,-174.5 }}},
    //                     {{{601.13,529.75,0},{514.15,596.81,-97}},{{512.67,596.81,-97},{454.15,596.81,-97 }}}};

    // cartesian_orientation={{{{0,0,-1.71},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{-0.65,0,-pi/2}},{{-0.65,0,-pi/2},{-0.78,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0.698,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{0.3,0,-pi/2}},{{0.3,0,-pi/2},{0.3,0,-pi/2}},{{0.3,0,-pi/2},{0.872,0,-pi/2}},{{0.872,0,-pi/2},{1,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0.65,0,-pi/2}},{{0.65,0,-pi/2},{0.78,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{-0.698,0,-pi/2}}},
    //                     {{{0,0,-pi/2},{-0.872,0,-pi/2}},{{-0.872,0,-pi/2},{-1,0,-pi/2}}}};


    cartesian_position={{{{601.13,529.75,0},{620.51,504,15}},{{620.51,480,15},{620.51,485, -10}}},
                        //{{{536.65,575.08,0},{538.65,564.98,1.66}},{{538.65,564.98 ,1.66},{500.65, 584.98, 1.66}}},
                        {{{536.65,575.08,0},{538.65,550,1.66}},{{538.65,550 ,1.66},{485.65, 580, 1.66}}},
                        
                        {{{536.65,575.08,0},{538.65,554.98,-0.34}},{{538.65,554.98,-0.34},{545,540,-115}},{{545 ,540 ,-110},{464.15, 550, -110}}},//{{536.65,575.08,0},{538.65,564.98,-0.34}}11.04新加B3
                        {{{594.66,571.27,0},{675.92,570,-9}},{{675.92,570,-9},{595.12,556.81,17}},{{595.12,556.81,17 },{511.059,638.158,166.16}}},//B4{{595.12,566.81,17 },{511.059,648.158,166.16}},D1{{594.66,571.27,0},{675.92,590,-9}}，D2{{675.92,570,-9},{595.12,556.81,17}}
                        {{{601.13,529.75,0},{690.15,550.81,97}},{{690.15,550.81,97},{595.12, 546.81,97}},{{595.12,546.81,97},{565.15,570,70}},{{565.15,565,70},{510.15,575,80}}},//D3{{601.13,529.75,0},{690.15,556.81,97}},D4{{690.15,556.81,97},{595.12, 556.81,97}}
                        {{{601.13,529.75,0},{620.51,485.98,-22.12}},{{620.51,485.98,-22.12},{620.51,485.98,10}}},
                        {{{536.65,575.08,0},{538.65,564.98,-1.66}},{{538.65,564.98,-1.66},{500.65, 584.98, -1.66}}},

                        {{{536.65,575.08,0},{538.65,569.98,0.34}},{{538.65,564.98,0.34},{525.15, 565.98, 110.84}},{{525.15, 565.98, 110.84},{484.15, 567.98,115.84}}},
                        {{{579.12,566.81,17 },{482.4,659.81,-154.5}}},
                        {{{601.13,529.75,0},{565.15,566.81,-85}},{{565.15,566.81,-85},{510.15,566.81,-80 }}}};

    cartesian_orientation={{{{0,0,-pi/2},{0,0,-pi/2-0.1}},{{0,0,-pi/2-0.1},{0,0,-pi/2-0.1}}},
                        {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0.1,0,-pi/2}}},
                        
                        {{{0,0,-pi/2},{0,0,-1.50}},{{0,0,-1.45},{-0.25,-0.1,-1.45}},{{-0.55,-0.1,-1.45},{-0.8,-0.2,-1.45}}},
                        {{{0,0.05,-pi/2},{0,0.05,-1.5}},{{0,0,-1.5},{0,0,-1.5}},{{0,0,-1.5},{0.647487,0.099531,-1.45421}}},//D1{{0,0,-pi/2},{0,0,-1.5}}
                        {{{0,0,-pi/2},{0.3,0,-1.45}},{{0.3,0,-1.45},{0.3,0,-1.5}},{{0.3,0,-1.5},{0.6,0.17,-1.55}},{{0.6,0.17,-1.55},{0.8,0.2,-1.5}}},
                        {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}}},
                        {{{0,0,-pi/2},{0,0,-1.45}},{{0,0,-1.45},{0,0,-1.5}}},

                        {{{0,0,-pi/2},{0,0,-1.45}},{{0,0,-1.45},{0.41,0.07,-1.47}},{{0.41,0.07,-1.47},{0.55,0.2,-1.45}}},
                        {{{0,0,-pi/2},{-0.63,-0.05,-pi/2}}},
                        {{{0,0,-pi/2},{-0.6,-0.2,-1.45}},{{-0.6,-0.2,-1.45},{-0.8,-0.2,-1.45}}}};
   
    Index_library.resize(7);
    for (int i=0;i<7;i++){
        Index_library[i].resize(4);
        for (int j=0;j<4;j++){
            Index_library[i][j].resize(2);
        }
    }

    //储存盖板与操作位的映射关系ca
    Index_library = {{{0,0},{0,1}},{{1,0},{1,1},{2,0},{3,2}},{{2,1},{2,2},{4,2},{4,3}},
                    {{3,0},{3,1},{4,0},{4,1}},{{9,0},{9,1},{7,1},{7,2}},/*{{7,0},{8,0},{6,0},{6,1}},*/ {{6,1},{6,0},{7,0},{8,0}},
                    {{5,0},{5,1}}};

    //储存操作位与ApriltagID的映射关系
    apriltags_search_library = {{0,1},{2,3},{4,8,9},{6,7,5},{10,11,12,13},{14,15},{16,17},{18,20,21},{19},{22,23}};

    motors = {(pi/2 - 0.01),0,pi/2,0,pi/2,0,0,0,0,0,0};

    Motors_continuous_check ={(pi/2 - 0.01),0,pi/2,0,pi/2,0,0,0,0,0,0};

    m_nh = nh;
    manualPattern=false;

    ros_widget = &w;
        
    init_finished=init();

    ROS_INFO("rosconnector init success!");
}

bool RosConnector::init() {
    
    if (!ros::master::check()) {
          return false;
    }
    
    m_Sub=m_nh.subscribe("/joy", 1, &RosConnector::call_back, this);//手柄

    simulation_image_Pub=m_nh.advertise<sensor_msgs::Image>("/usb_cam/image_raw",1);
    simulation_info_Pub=m_nh.advertise<sensor_msgs::CameraInfo>("/usb_cam/camera_info",1);
    apriltag_corner_point=m_nh.subscribe("/corner_point", 1, &RosConnector::apriltag_call_back, this);//接收角点
    apriltag_tag_detections=m_nh.subscribe("/tag_detections", 1, &RosConnector::apriltag_detections_call_back, this);//接收apriltag位姿

    m_Pub_Port4=m_nh.advertise<dynamixel_sdk_examples::BulkSetItem_Port4>("/bulk_set_item_port4", 1);//关节舵机
    m_Pub_Port3=m_nh.advertise<dynamixel_sdk_examples::BulkSetItem_Port3>("/bulk_set_item_port3", 1);//推杆舵机
    m_Pub_M=m_nh.advertise<std_msgs::Float32MultiArray>("/Maxon/command/MotorPosition", 1);//丝传动maxon电机
    // Pub_total_position=m_nh.advertise<std_msgs::Float32MultiArray>("/Simulation/command/JointPosition",1);//全部发送，用于仿真

    bulk_Client_Port4=m_nh.serviceClient<dynamixel_sdk_examples::BulkGetItem_Port4>("/bulk_get_item_port4");
    bulk_Client_Port3=m_nh.serviceClient<dynamixel_sdk_examples::BulkGetItem_Port3>("/bulk_get_item_port3");
    maxon_Client=m_nh.serviceClient<epos_msgs::MotorStatesSRV>("/Maxon/state/server");
    return true;
}

//接收手柄信息
void RosConnector::call_back(const sensor_msgs::Joy &msg){
    if(manualPattern){
        get_cartesian(msg);

        if(msg.buttons[1]){
            button_3_check = true;
        }
        if(!msg.buttons[1] && button_3_check)
        {
            manualPattern=!manualPattern;
        }

        pub_all_robot(motors);
        // pub_total_position(motors,i_operate); //仿真中控制关节速度   

        if(limited_joint_outsend == 0){
            m_param_record = m_param;
        }else{
            m_x = m_param_record[0];
            m_y = m_param_record[1];
            m_z = m_param_record[2];
            m_thx = m_param_record[3];
            m_thy = m_param_record[4];
            m_thz = m_param_record[5];
            std::cout<<"末端位姿"<<m_x<<"  "<<m_y<<"  "<<m_z<<" "<<m_thx<<"  "<<m_thy<<"  "<<m_thz<<std::endl;
          
            // kine.Param_writeto_Inv_sol(m_param_record);
            // motors_load=kine.Inv_sol_addh(i_operate);
            
            // std::cout<<"关节角"<<motors_load[0]<<"  "<<motors_load[1]<<"  "<<motors_load[2]<<"  "<<motors_load[3]<<"  "<<motors_load[4]<<"  "<<motors_load[5]<<"  "<<motors_load[10]<<std::endl;
            
        }

        check_direct();
    }
    
    if(!manualPattern){
        button_fit_check(msg);
    }
}

void RosConnector::button_fit_check(const sensor_msgs::Joy &msg){
    if(!off_touch && !msg.buttons[5] && !msg.buttons[3] && !msg.buttons[1] && !msg.buttons[4]){
        off_touch = true;
    }
    if(off_touch){
        if(msg.buttons[4]){
            button_check_right = 1;
        }
        if(msg.buttons[5]){
            button_check_left = 1;
        }
        if(msg.buttons[1]){
            button_check_cot = 1;
        }
        if(msg.buttons[3]){
            button_check_cot = -1;
        }
        if(button_check_right == 1 && !msg.buttons[4]){
            std::cout<<"Yes"<<endl;
            button_check_right = 0;
            button_check_Yes_No = 1;
            button_check_continue = 7;
        }
        if(button_check_left == 1 && !msg.buttons[5]){
            std::cout<<"No"<<endl;
            button_check_left = 0;
            button_check_Yes_No = 1;
            button_check_continue = 13;
        }
        if(button_check_cot == 1 && !msg.buttons[1]){
            std::cout<<"Continue"<<endl;
            button_check_cot = 0;
            button_check_continue = 1;
        }
        if(button_check_cot == -1 && !msg.buttons[3]){
            std::cout<<"Continue"<<endl;
            button_check_cot = 0;
            button_check_continue = 2;
        }
        if(button_check_cot == 1){
            button_check_long_push ++ ;
        }else{
            button_check_long_push = 0;
        }
        if(button_check_long_push > 300){
            std::cout<<"Joints Back!"<<endl;
            button_check_continue = -1;
        }
    }

    {//微动开关检测套上
        if(msg.buttons[0]){
            screw_catch_button = true;
        }
        if(!msg.buttons[0]){
            screw_catch_button = false;
        }    
    }
}

void RosConnector::simulation_image_publish(){
    if(ros_widget->image_usbcam.height() != 0){

        header.frame_id = "usb_cam";
        header.stamp = ros::Time::now();
        opencv_usbcam = cv::Mat(ros_widget->image_usbcam.height(), ros_widget->image_usbcam.width(), CV_8UC3, const_cast<uchar*>(ros_widget->image_usbcam.bits()), ros_widget->image_usbcam.bytesPerLine()).clone();
        image_to_april = cv_bridge::CvImage(header, "rgb8", opencv_usbcam).toImageMsg();

        simulation_image_Pub.publish(image_to_april);
        info_to_april.header.seq = image_to_april->header.seq;
        info_to_april.header.stamp = image_to_april->header.stamp;
        simulation_info_Pub.publish(info_to_april);
        // simulation_display_Pub.publish(image_to_display);
    }

}

void RosConnector::apriltag_call_back(const std_msgs::Float32MultiArray &msg){

    for(int i=0; i<32; i++){
        apriltag_corpoint[i] = msg.data[i];
        ros_widget->apriltags_realtimepaint[i] = msg.data[i];
    }
}
/*
void RosConnector::apriltag_detections_call_back(const apriltag_ros::AprilTagDetectionArray &msg){
    
    if(msg.detections.size() != 0){
        for(int i=0; i<msg.detections.size(); i++){
            if(msg.detections[i].id[0] == apriltags_search_library[i_operate][i_screw]){
                apriltag_position_posture[0] = msg.detections[i].pose.pose.pose.position.x;
                apriltag_position_posture[1] = msg.detections[i].pose.pose.pose.position.y;
                apriltag_position_posture[2] = msg.detections[i].pose.pose.pose.position.z;
                apriltag_position_posture[3] = msg.detections[i].pose.pose.pose.orientation.x;
                apriltag_position_posture[4] = msg.detections[i].pose.pose.pose.orientation.y;
                apriltag_position_posture[5] = msg.detections[i].pose.pose.pose.orientation.z;
                apriltag_position_posture[6] = msg.detections[i].pose.pose.pose.orientation.w;
                continue;
            }
        }
    }else{
        apriltag_position_posture[6] = 0;
        ros_widget->apriltags_realtimepaint[0] = 0;
    }
    // std::cout<<"apriltag_position_posture"<<apriltag_position_posture[0]<<std::endl;
}
*/


void RosConnector::apriltag_detections_call_back(const apriltag_ros::AprilTagDetectionArray &msg){
    
    if(msg.detections.size() != 0){
        float projtction_z = 0;  //z轴线投影
        int max_index = 0;       //投影最大对应索引
        for(int i=0; i<msg.detections.size(); i++){
            if(msg.detections[i].id[0] == apriltags_search_library[i_operate][i_screw]){
                float temp_projection_z = abs(1-2*msg.detections[i].pose.pose.pose.orientation.x*msg.detections[i].pose.pose.pose.orientation.x-2*msg.detections[i].pose.pose.pose.orientation.y*msg.detections[i].pose.pose.pose.orientation.y);
                if(projtction_z < temp_projection_z)
                {
                    projtction_z = temp_projection_z;
                    max_index = i;
                }
            }
        }
        if(projtction_z != 0){
            apriltag_position_posture[0] = msg.detections[max_index].pose.pose.pose.position.x;
            apriltag_position_posture[1] = msg.detections[max_index].pose.pose.pose.position.y;
            apriltag_position_posture[2] = msg.detections[max_index].pose.pose.pose.position.z;
            apriltag_position_posture[3] = msg.detections[max_index].pose.pose.pose.orientation.x;
            apriltag_position_posture[4] = msg.detections[max_index].pose.pose.pose.orientation.y;
            apriltag_position_posture[5] = msg.detections[max_index].pose.pose.pose.orientation.z;
            apriltag_position_posture[6] = msg.detections[max_index].pose.pose.pose.orientation.w;
        }else{
            apriltag_position_posture[6] = 0;
            ros_widget->apriltags_realtimepaint[0] = 0;
        }
    }else{
        apriltag_position_posture[6] = 0;
        ros_widget->apriltags_realtimepaint[0] = 0;
    }
    // std::cout<<"apriltag_position_posture:  "<<apriltag_position_posture[0]<<"  "<<apriltag_position_posture[1]<<"  "<<apriltag_position_posture[2]<<"  "<<apriltag_position_posture[6]<<std::endl;
}


//手柄移动增量
void RosConnector::get_cartesian(const sensor_msgs::Joy &msg)
{
    if(msg.buttons[2]){
        chg_target = 1;
    }
    if(!msg.buttons[2] && chg_target == 1){
        chg_for_control = 1 - chg_for_control;
        chg_target = 0;
    }

    //位置增量参数
    double position_increment = 10;
    //姿态增量参数
    double posture_increment = 500;


    if(chg_for_control == 1){
        m_param = {m_x,m_y,m_z,m_thx,m_thy,m_thz};
        kine.Param_writeto_Inv_sol(m_param);
        // if (i_operate == 1 || i_operate == 2 || i_operate == 6 || i_operate == 7){
        //     motors=kine.Inv_sol_addh(i_operate);
        // }else{
            motors=kine.Inv_sol(i_operate, i_screw );
            kine.Param_joints_update_simulation_for(motors);
        // }
        //位姿分离关节独立
        //m_thz_chg = msg.axes[1];
        m_thz_chg = msg.buttons[9]-msg.buttons[7];
        
        motors[4] = - m_thz_chg/posture_increment + motors[4];//z是俯仰
       // m_thx_chg = msg.axes[0];
        // m_thx_chg = msg.buttons[6]-msg.buttons[7];
          m_thx_chg = double (msg.buttons[11])-double (msg.buttons[10]);
        motors[3] = - m_thx_chg/posture_increment + motors[3];//x是偏摆
        m_thy_chg = double(msg.buttons[8])-double(msg.buttons[6]);
        motors[5] = m_thy_chg/posture_increment + motors[5];//y是自转
        //笛卡尔
        // m_thz_chg = msg.axes[1];
        // m_thz = - m_thz_chg/posture_increment + m_thz;//z是俯仰
        // m_thx_chg = msg.axes[0];
        // m_thx = - m_thx_chg/posture_increment + m_thx;//x是自转
        // m_thy_chg = (double(msg.buttons[11])-double(msg.buttons[9]));
        // m_thy = m_thy_chg/posture_increment + m_thy;//y是偏摆

        // m_param = {m_x,m_y,m_z,m_thx,m_thy,m_thz};

        // kine.Param_writeto_Inv_sol(m_param);

        // if (i_operate == 1 || i_operate == 2 || i_operate == 6 || i_operate == 7){
        //     motors_load=kine.Inv_sol_addh(i_operate);
        // }else{
        //     motors_load=kine.Inv_sol(i_operate, i_screw );
        // }
        // motors[0] = motors_load[0];
        // motors[1] = motors_load[1];
        // motors[2] = motors_load[2];
        // motors[3] = motors_load[3];
        // motors[4] = motors_load[4];
        // motors[5] = motors_load[5];
        // motors[6] = motors_load[6];
        // motors[7] = motors_load[7];
        // motors[8] = motors_load[8];
        // motors[9] = motors_load[9];
        // motors[10] = motors_load[10];
        //笛卡尔
    }else if(chg_for_control == 0){
       // m_z_chg = msg.axes[0];
        // m_thx_chg = msg.buttons[6]-msg.buttons[7];
        m_x_chg = double (msg.buttons[7])-double (msg.buttons[9]);
       // m_z = m_z_chg/position_increment + m_z;//z是左右
       // m_x_chg = msg.axes[1];
        m_z_chg = msg.buttons[11]-msg.buttons[10];
         m_z = m_z_chg/position_increment + m_z;//z是左右
        m_x = m_x_chg/position_increment + m_x;//x是前后
        //m_y_chg = (double(msg.buttons[9])-double(msg.buttons[8])) + (double(msg.buttons[10])-double(msg.buttons[11]));
        m_y_chg = double(msg.buttons[6])-double(msg.buttons[8]);
        m_y = - m_y_chg/position_increment + m_y;//y是上下

        m_param = {m_x,m_y,m_z,m_thx,m_thy,m_thz};

        kine.Param_writeto_Inv_sol(m_param);

        // if (i_operate == 1 || i_operate == 2 || i_operate == 6 || i_operate == 7){
        //     motors_load=kine.Inv_sol_addh(i_operate);
        // }else{
            motors_load=kine.Inv_sol(i_operate, i_screw );
            
            kine.Param_joints_update_simulation_for(motors_load);
        // }
        motors[0] = motors_load[0];
        motors[1] = motors_load[1];
        motors[2] = motors_load[2];
        //锁定姿态关节
        // motors[3] = motors_load[3];
        // motors[4] = motors_load[4];
        // motors[5] = motors_load[5];
        //锁定姿态关节
        motors[6] = motors_load[6];
        motors[7] = motors_load[7];
        motors[8] = motors_load[8];
        motors[9] = motors_load[9];
        motors[10] = motors_load[10];
    }

    cout<<"手动1关节："<<motors[0]<<" "<<"手动升降："<<motors[10]<<endl;

    kine.Param_joints_update_simulation(motors);
    if (i_operate == 1 || i_operate == 2 || i_operate == 6 || i_operate == 7){
        m_param = kine.kec_joints2End_addh(i_operate);
    }else{
        m_param = kine.kec_joints2End();
    }
 
    std::cout<<"x:"<<m_param[0]<<"  y:"<<m_param[1]<<"  z:"<<m_param[2]<<"  "<<m_param[3]<<"  thy:"<<m_param[4]<<"  thz:"<<m_param[5]<<std::endl;

    std::ofstream ofs1;
    ofs1.open("/home/nuc/qt_file2/position.txt");
    ofs1<<"  "<<m_param[0]<<" "<<m_param[1]<<"  "<<m_param[2]<<"  "<<m_param[3]<<"  "<<m_param[4]<<"  "<<m_param[5]<<std::endl;
    ofs1.close();

    m_x = m_param[0];
    m_y = m_param[1];
    m_z = m_param[2];
    m_thx = m_param[3];
    m_thy = m_param[4];
    m_thz = m_param[5];

    //m_param.erase();
}




//为所有关节舵机发布弧度或弧度每秒
void RosConnector::pub_all_robot(/*const */vector<double> &motor_values){
    ros::Rate loop_rate(rate_f);

    if(lift_start == 0){

        vector<double> motor_offset = present_position_get();
        lift_offset = motor_offset[10];

        // lift_offset = 0;

        lift_start = 1;
        std::ofstream ofs;
        ofs.open("/home/nuc/qt_file2/intco.txt");
        ofs<<lift_offset;
        ofs.close();
    }

    for(int i=0;i<5;i++)
    {
        if(isnan(motor_values[i]))
        {
           std::cout<<"关节无理论值"<<endl;
            loop_rate.sleep();
            return;
                
        }
    }

    if(abs(motor_values[0]-Motors_continuous_check[0])>0.1 || abs(motor_values[1]-Motors_continuous_check[1])>0.1 || abs(motor_values[2]-Motors_continuous_check[2])>0.1 ||abs(motor_values[3]-Motors_continuous_check[3])>0.1 || abs(motor_values[4]-Motors_continuous_check[4])>0.1 || abs(motor_values[5]-Motors_continuous_check[5])>0.1||
           abs(motor_values[6]-Motors_continuous_check[6])>0.1 || abs(motor_values[7]-Motors_continuous_check[7])>0.1 || abs(motor_values[8]-Motors_continuous_check[8])>0.1 || abs(motor_values[9]-Motors_continuous_check[9])>0.1 || abs(motor_values[10]-Motors_continuous_check[10])>5 
           || !check_limit(motor_values))
    {
            if(!check_limit(motor_values)){
                cout<<"overrun"<<endl;
            }else{
                cout<<"出现关节角突变点"<<endl;
            }
            motor_values = Motors_continuous_check; //强制保持motors在限位
            //std::cout<<"电机转动角度"<<maxon_msg.data[0]<<"  "<<maxon_msg.data[1]<<"  "<<maxon_msg.data[2]<<"  "<<maxon_msg.data[3]<<std::endl;
            //std::cout<<"舵机转动角度"<<motor_values[0]<<"  "<<motor_values[1]<<"  "<<motor_values[2]<<"  "<<motor_values[3]<<"  "<<motor_values[4]<<"  "<<motor_values[5]<<"  "<<motor_values[10]<<std::endl;
            // limited_joint = 0;
    }
    else
    {
            Motors_continuous_check = motor_values;

            bulk_msg_port4.value1 = int(-motor_values[10] / (18*pi)*4096 + lift_offset);
            bulk_msg_port4.value2 = int(-motor_values[0] / (2 * pi) * 4096 + 3072); //此处改为2048
            
            // bulk_msg_port3.value1 = int(motor_values[3] / (2 * pi) * 4096 + 2048);
            //bulk_msg_port3.value2 = int(motor_values[4] / (2 * pi) * 4096 + 1024);

            bulk_msg_port3.value1 = int(-motor_values[3] / (2 * pi) * 4096 + 2048);
            bulk_msg_port3.value2 = int(-motor_values[4] / (2 * pi) * 4096 + 2048+1024);
            
           //cout<<"关节"<<motor_values[3]<<"  "<<motor_values[4]<<endl;
           // cout<< "4 5 舵机"<<bulk_msg_port3.value1<<"  "<<bulk_msg_port3.value2<<endl;



            bulk_msg_port3.value3 = int(motor_values[5] / (2 * pi) * 4096 + 2048 + 341);//Compensated machining offset round/16
            
            // maxon_msg.data[0]=motor_values[6];
            // maxon_msg.data[1]=motor_values[7];
            // maxon_msg.data[2]=motor_values[8];
            // maxon_msg.data[3]=motor_values[9];
                maxon_msg.data[0]=motor_values[6]+maxon_reset[0];
                maxon_msg.data[1]=motor_values[7]+maxon_reset[1];
                maxon_msg.data[2]=motor_values[8]+maxon_reset[2];
                maxon_msg.data[3]=motor_values[9]+maxon_reset[3];
        std::cout<<"电机转动角度"<<maxon_msg.data[0]<<"  "<<maxon_msg.data[1]<<"  "<<maxon_msg.data[2]<<"  "<<maxon_msg.data[3]<<std::endl;
        //std::cout<<"舵机转动角度"<<motor_values[0]<<"  "<<motor_values[3]<<"  "<<motor_values[4]<<"  "<<motor_values[5]<<std::endl;


        if(check_limit(motor_values)){
            m_Pub_Port4.publish(bulk_msg_port4);
            m_Pub_Port3.publish(bulk_msg_port3);
            m_Pub_M.publish(maxon_msg);
        }
    }
          

    loop_rate.sleep();
}

//根据电机位置和电流调整发送给电机的量
void RosConnector::adjustmotor(const vector<double>& motor_values) 
{

    
    ros::Rate loop_rate(rate_f);
   
        for (int a = 0; a < 4; a++) {
             if (motor_values[a + 6]- record_maxon_pos[a]>0)//大于0电机放丝
             {
                         
                     maxon_reset[a] = maxon_reset[a] - 0.1* (motor_values[a + 6] - record_maxon_pos[a]);//赋给当前时刻驱动丝
                   // std::cout<<"放丝： "<<a<<"  "<<maxon_reset[a]<<endl;

                  //  pub_all_robot(motor_values);
             }
             else if(maxon_reset[a] < 0)
             {
                 maxon_reset[a] = maxon_reset[a] - 0.1*(motor_values[a + 6] - record_maxon_pos[a]);
                 
                 std::cout<<"收丝： "<<a<<"  "<<maxon_reset[a]<<endl;
                 //maxon_reset[a] = 0;
             }
        }


    for (int index = 0; index < 4; index++) {
        record_maxon_pos[index] = motor_values[index + 6];
    }

    loop_rate.sleep();
}


//为仿真环境发布所有电机的弧度
// void RosConnector::pub_total_position(const vector<double> &msg, int work_target){
//     total_msg.data[0] = msg[0];
//     total_msg.data[1] = msg[1];
//     total_msg.data[2] = msg[2];
//     total_msg.data[3] = msg[3];
//     total_msg.data[4] = msg[4];
//     total_msg.data[5] = msg[5];
//     total_msg.data[6] = msg[10];
//     total_msg.data[7] = work_target+1;
//     Pub_total_position.publish(total_msg);
// }

  //根据手柄信息获取方向
void RosConnector::check_direct(){

    if (limited_joint == 0)
    {
        QString direct_x;
        QString direct_y;
        QString direct_z;
        QString direct_thx;
        QString direct_thy;
        QString direct_thz;

        QString direct_2;
        QString direct_3;

        if (chg_for_control == 0){
            if (m_x_chg < 0){
                direct_x = QString("后");
            }else if (m_x_chg > 0){
                direct_x = QString("前");
            }else{
                direct_x = QString("&nbsp;&nbsp;&nbsp;&nbsp;");
            }
            if (m_y_chg < 0){
                direct_y = QString("下");
            }else if (m_y_chg > 0){
                direct_y = QString("上");
            }else{
                direct_y = QString("&nbsp;&nbsp;&nbsp;&nbsp;");
            }
            if (m_z_chg < 0){
                direct_z = QString("右");
            }else if (m_z_chg > 0){
                direct_z = QString("左");
            }else{
                direct_z = QString("&nbsp;&nbsp;&nbsp;&nbsp;");
            }
            direct_2 = QString("位置:");
            direct_3 = direct_x + direct_y + direct_z;
        }
        else
        {
            if (m_thx_chg < 0){
                direct_thx = QString("右");
            }else if (m_thx_chg > 0){
                direct_thx = QString("左");
            }else{
                direct_thx = QString("&nbsp;&nbsp;&nbsp;&nbsp;");
            }
            if (m_thy_chg > 0){
                direct_thy = QString("逆");
            }else if (m_thy_chg < 0){
                direct_thy = QString("顺");
            }else{
                direct_thy = QString("&nbsp;&nbsp;&nbsp;&nbsp;");
            }
            if (m_thz_chg < 0){
                direct_thz = QString("仰");
            }else if (m_thz_chg > 0){
                direct_thz = QString("俯");
            }else{
                direct_thz = QString("&nbsp;&nbsp;&nbsp;&nbsp;");
            }
            direct_2 = QString("姿态:");
            direct_3 = direct_thx + direct_thy + direct_thz;
        }

        if (limited_warning_up != 0){


            QString direct_1 = QString("<font color = #00FF00>%1</font> <font color = red>%2</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = red>%3</font> <font color = orange>%4</font> <font color = red>%5</font>")
                            .arg(direct_2,direct_3,joints_namespace[limited_warning_up],"关节接近边界",adjust_up[limited_warning_up]);
            ros_widget->UIsetText_top(direct_1);



        }
        else if(limited_warning_down != 0)
        {
            QString direct_1 = QString("<font color = #00FF00>%1</font> <font color = red>%2</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = red>%3</font> <font color = orange>%4</font> <font color = red>%5</font>")
                            .arg(direct_2,direct_3,joints_namespace[limited_warning_down],"关节接近边界",adjust_down[limited_warning_down]);
            ros_widget->UIsetText_top(direct_1);
        }
        else
        {
            QString direct_1 = QString("<font color = #00FF00>%1</font> <font color = red>%2</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = #00FF00>%3</font>")
                            .arg(direct_2,direct_3,"处于安全范围中");
            ros_widget->UIsetText_top(direct_1);
        }  
    }
    else
    {
        QString direct_1 = QString("<font color = red>%1</font> <font color = red>%2</font> <font color = red>%3</font>")
                            .arg(joints_namespace[limited_joint],"关节","已超限");
        ros_widget->UIsetText_top(direct_1);
    }
}


//读取所有电机当前位置
vector<double> RosConnector::present_position_get(){

    bulk_Port4.request.id1 = 1;
    bulk_Port4.request.id2 = 2;
    bulk_Port3.request.id1 = 3;
    bulk_Port3.request.id2 = 4;
    bulk_Port3.request.id3 = 5;
    bulk_Port4.request.item1 = "position";
    bulk_Port4.request.item2 = "position";
    bulk_Port3.request.item1 = "position";
    bulk_Port3.request.item2 = "position";
    bulk_Port3.request.item3 = "position";
    maxon_srv.request.name="maxon";
    
    bulk_Client_Port3.call(bulk_Port3);
    
    present_position[3] = double(bulk_Port3.response.value1 - 2048)/4096 * 2 * pi;
    present_position[4] = double(bulk_Port3.response.value2 - 1024)/4096 * 2 * pi;
    present_position[5] = double(bulk_Port3.response.value3 - 2048 - 341)/4096 * 2 * pi;

    maxon_Client.call(maxon_srv);

    present_position[9] = -maxon_srv.response.motors.data[3];
    present_position[8] = maxon_srv.response.motors.data[1];
    present_position[7] = maxon_srv.response.motors.data[2];
    present_position[6] = -maxon_srv.response.motors.data[0];
    
    bulk_Client_Port4.call(bulk_Port4);
    present_position[0] = double(3072 - bulk_Port4.response.value2)/4096 * 2 * pi;
    present_position[10] = bulk_Port4.response.value1;
    std::cout<<"编码器读取: "<<bulk_Port4.response.value1<<"  "<<bulk_Port4.response.value2<<"  "<<bulk_Port3.response.value1<<"  "<<bulk_Port3.response.value2<<"  "<<bulk_Port3.response.value3<<std::endl;
    std::cout<< "编码器读取"<<present_position[3]<<" "<< present_position[4]<<" "<< present_position[5]<<" "<< present_position[0]<<" "<< present_position[10]<<" " <<endl;
    return present_position;
}

//设定手动模式的初始位置
void RosConnector::set_manual_start(const vector<double> &position){
    m_x = position[0];
    m_y = position[1]; 
    m_z = position[2];
    m_thx = position[3];
    m_thy = position[4];
    m_thz = position[5]; 
}

//check joint limit 
bool RosConnector::check_limit(const vector<double> &motors_check)
{
    limited_joint = 0;
    limited_joint_outsend = 0;
    limited_warning_up = 0;
    limited_warning_down = 0;

    for(int i=0;i<motor_limit_value.size();++i)
    {
        if(motors_check[i]>=motor_limit_value[i][1]*pi/180 || motors_check[i]<=motor_limit_value[i][0]*pi/180){
            ROS_INFO("------------joint %d is out of range!!!!---------",i+1);
            limited_joint = i+1;
            limited_joint_outsend = limited_joint;
        }
        
        else if((motors_check[i]-(motor_limit_value[i][0]*pi/180)) < (((motor_limit_value[i][1] - motor_limit_value[i][0])*pi/180)* 0.1))
        {
            limited_warning_down = i + 1;
        }
        else if(((motor_limit_value[i][1]*pi/180)-motors_check[i]) < (((motor_limit_value[i][1] - motor_limit_value[i][0])*pi/180)* 0.1))
        {
            limited_warning_up = i + 1;
        }
    }

    if(motors_check[10]>0.1 && motors_check[10]<-130){
        limited_joint = 7;
        limited_joint_outsend = limited_joint;
    }else if(motors_check[10]+130<130.1*0.1){
        limited_warning_down=7;
    }else if(0.1-motors_check[10]<130.1*0.1){
        limited_warning_up=7;
    }

    if(limited_joint == 0){
        return true;
    }else{
        return false;
    }
}

RosConnector::~RosConnector(){
    if(ros::isStarted()) {
            ros::shutdown();
            ros::waitForShutdown();
        }
}

